#include "ti_msp_dl_config.h"
#include "Driver/hardware/ICM20948/ICM20948.h"
#include "Driver/hardware/ICM20948/DMP/icm20948_dmp_driver.h"  // 添加DMP头文件
#include "Driver/hardware/OLED/sh1106.h"
#include "Driver/software/UART_XDS_PC/UART_XDS_PC.h"
#include "Driver/software/UART-4G/UART_4G.h"  // 添加4G模块头文件
#include "Driver/software/OneNet/onenet_data.h"  // 添加OneNet头文件
#include "Driver/CLOCK/clock.h"
#include "Driver/hardware/motor/motor.h" // 添加电机头文件
#include "Driver/software/PID/pid.h"     // 添加PID头文件
#include <stdio.h>
#include <math.h>
#include <string.h>

// 欧拉角数据结构
typedef struct {
    float roll;     // 横滚角（绕X轴）
    float pitch;    // 俯仰角（绕Y轴）
    float yaw;      // 偏航角（绕Z轴）
} EulerAngles_t;

// 传感器校准数据
typedef struct {
    float accel_x_offset;
    float accel_y_offset;
    float accel_z_offset;
    float gyro_x_offset;
    float gyro_y_offset;
    float gyro_z_offset;
    float roll_offset;
    float pitch_offset;
    float yaw_offset;
    bool calibrated;
} SensorCalibration_t;

// 全局变量
SensorCalibration_t g_calibration = {0};
bool g_use_dmp = true;  // 是否使用DMP功能
uint32_t g_last_upload_time = 0;  // 上次上传数据的时间
volatile uint32_t g_uart2_irq_count = 0; // !! 用于诊断UART2中断是否触发的计数器 !!

// 为偏航角（方向）控制定义的PID控制器
PID_Controller yaw_pid;
float yaw_setpoint = 0.0f; // 目标偏航角，用于保持直线行驶
bool yaw_setpoint_initialized = false; // 偏航角设定点是否已初始化
const uint16_t base_speed = 250; // 小车基础前进速度 (0-640)


// 函数声明
void SystemInit(void);
void CalibrateSensors(void);
void CalculateEulerAngles(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles);
void DisplaySensorData(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles);
void DisplayStartupScreen(void);
void DisplayCalibrationScreen(void);
void HandleDMPData(EulerAngles_t *eulerAngles);
void SendDataToOneNet(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles);
void OneNet_SendCallback(const char *json_data);
void UpdateYawSteeringControl(EulerAngles_t *eulerAngles);
void OneNet_Command_Handler(const char *data);

int main() {
    // 系统初始化
    SystemInit();
    
    // 显示启动画面
    DisplayStartupScreen();
    
    // 传感器校准
    CalibrateSensors();
    
    // 传感器数据结构
    ICM20948_Data_t sensorData;
    EulerAngles_t eulerAngles = {0};
    char debugBuffer[128];
    
    // 初始化DMP功能
    if (g_use_dmp) {
        if (ICM20948_DMP_Init(ICM20948_DMP_FEATURE_6X_QUAT | ICM20948_DMP_FEATURE_GYRO_CAL) == 0) {
            ICM20948_DMP_Enable(true);
            UART_XDS_PC_DefaultSendLine("DMP initialized and enabled");
            
            // 显示DMP模式信息
            SH1106_Clear();
            SH1106_DrawString(15, 1, "DMP Enabled", FONT_SIZE_6x8);
            SH1106_DrawString(10, 3, "Using 6-axis fusion", FONT_SIZE_6x8);
            SH1106_DrawString(15, 5, "Starting...", FONT_SIZE_6x8);
            SH1106_Display();
            mspm0_delay_ms(2000);
        } else {
            g_use_dmp = false;
            UART_XDS_PC_DefaultSendLine("DMP initialization failed, falling back to software fusion");
        }
    }
    
    // 主循环
    while (1) {
        if (g_use_dmp) {
            // 使用DMP处理姿态
            HandleDMPData(&eulerAngles);
            
            // 依然读取原始传感器数据用于显示
            ICM20948_ReadData(&sensorData);
        } else {
            // 使用软件融合计算姿态
            ICM20948_ReadData(&sensorData);
            
            // 应用校准偏移
            sensorData.accel_x_g -= g_calibration.accel_x_offset;
            sensorData.accel_y_g -= g_calibration.accel_y_offset;
            sensorData.accel_z_g -= g_calibration.accel_z_offset;
            sensorData.gyro_x_dps -= g_calibration.gyro_x_offset;
            sensorData.gyro_y_dps -= g_calibration.gyro_y_offset;
            sensorData.gyro_z_dps -= g_calibration.gyro_z_offset;
            
            // 计算欧拉角
            CalculateEulerAngles(&sensorData, &eulerAngles);
        }
        
        // 根据PID输出更新电机控制，以保持直线行驶
        UpdateYawSteeringControl(&eulerAngles);
        
        // 显示数据到OLED
        DisplaySensorData(&sensorData, &eulerAngles);
        
        // 通过串口输出调试信息
        snprintf(debugBuffer, sizeof(debugBuffer), 
                 "%s: Roll:%.2f Pitch:%.2f Yaw:%.2f IRQ:%u\r\n", 
                 g_use_dmp ? "DMP" : "SW",
                 eulerAngles.roll, eulerAngles.pitch, eulerAngles.yaw, g_uart2_irq_count);
        UART_XDS_PC_DefaultSendString(debugBuffer);
        
        // 每秒上传一次数据到OneNet
        if (tick_ms - g_last_upload_time >= 1000) {
            SendDataToOneNet(&sensorData, &eulerAngles);
            g_last_upload_time = tick_ms;
        }
        
        // 延时刷新，控制更新频率
        mspm0_delay_ms(100);
    }
}


/**
 * @brief UART2中断服务程序 (4G/GPS模块)
 * @note 此函数负责响应UART2的接收中断，并调用4G模块的处理器。
 */
void UART2_IRQHandler(void)
{
    g_uart2_irq_count++; // 中断触发，计数器加一
    switch (DL_UART_Main_getPendingInterrupt(UART_4G_GPS_INST)) {
        case DL_UART_MAIN_IIDX_RX:
            // 调用4G模块的接收处理器
            UART_4G_IRQHandler();
            break;
            
        default:
            break;
    }
}


/**
 * @brief 发送数据到OneNet
 * @param sensorData 传感器数据
 * @param eulerAngles 欧拉角数据
 */
void SendDataToOneNet(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles) {
    // 获取新的消息ID
    uint32_t message_id = OneNet_GetNextMessageId();
    
    // 使用组合发送多个属性
    OneNet_Status_t status = OneNet_SendMultipleProperties(
        message_id,
        "%f%f%f%f",
        ONENET_PROP_ROLL, eulerAngles->roll,
        ONENET_PROP_PITCH, eulerAngles->pitch,
        ONENET_PROP_YAW, eulerAngles->yaw,
        ONENET_PROP_TEMP, sensorData->temp_c
    );
    
    // 显示发送状态
    if (status == ONENET_STATUS_SUCCESS) {
        UART_XDS_PC_DefaultSendLine("OneNet data sent successfully");
    } else {
        UART_XDS_PC_DefaultSendLine("OneNet data send failed");
    }
}

/**
 * @brief OneNet发送回调函数，将数据发送到4G模块
 * @param json_data JSON格式的数据
 */
void OneNet_SendCallback(const char *json_data) {
    // 通过4G模块发送JSON数据
    UART_4G_SendString(json_data);
    
    // 调试打印发送的数据
    UART_XDS_PC_DefaultSendLine("Sending to OneNet:");
    UART_XDS_PC_DefaultSendLine(json_data);
}

/**
 * @brief 处理DMP数据
 * @param eulerAngles 欧拉角结构体指针
 */
void HandleDMPData(EulerAngles_t *eulerAngles) {
    ICM20948_DMP_EulerAngles_t dmpAngles;
    
    // 从DMP读取欧拉角数据
    if (ICM20948_DMP_ReadEulerAngles(&dmpAngles) == 0) {
        // 应用校准偏移
        eulerAngles->roll = dmpAngles.roll - g_calibration.roll_offset;
        eulerAngles->pitch = dmpAngles.pitch - g_calibration.pitch_offset;
        eulerAngles->yaw = dmpAngles.yaw - g_calibration.yaw_offset;
    }
}

/**
 * @brief 处理从OneNet云端接收到的指令
 * @param data 接收到的JSON格式的字符串数据
 */
void OneNet_Command_Handler(const char *data)
{
    UART_XDS_PC_DefaultSendLine("Received command from OneNet:");
    UART_XDS_PC_DefaultSendLine(data);

    // 这是一个非常简单的JSON解析，用于演示目的
    // 在生产环境中，建议使用功能完善的JSON解析库
    const char *id_key = "\"id\":\"";
    char *id_start = strstr(data, id_key);
    char request_id[32] = {0};

    if (id_start) {
        id_start += strlen(id_key);
        char *id_end = strchr(id_start, '"');
        if (id_end && (id_end - id_start < sizeof(request_id))) {
            strncpy(request_id, id_start, id_end - id_start);
        }
    } else {
        UART_XDS_PC_DefaultSendLine("Warning: 'id' not found in command. Cannot send response.");
    }

    // 解析并执行命令
    const char *stop_cmd = "\"command\":\"stop\"";
    const char *forward_cmd = "\"command\":\"forward\"";
    const char *backward_cmd = "\"command\":\"backward\"";
    const char *left_cmd = "\"command\":\"turn_left\"";
    const char *right_cmd = "\"command\":\"turn_right\"";

    bool cmd_executed = true; // 默认为true，除非命令未知

    if (strstr(data, stop_cmd)) {
        motor_set_right_speed(MOTOR_DIRECTION_FORWARD, 0);
        motor_set_left_speed(MOTOR_DIRECTION_FORWARD, 0);
        UART_XDS_PC_DefaultSendLine("Executing: STOP");
    } else if (strstr(data, forward_cmd)) {
        motor_set_right_speed(MOTOR_DIRECTION_FORWARD, base_speed);
        motor_set_left_speed(MOTOR_DIRECTION_FORWARD, base_speed);
        UART_XDS_PC_DefaultSendLine("Executing: FORWARD");
    } else if (strstr(data, backward_cmd)) {
        motor_set_right_speed(MOTOR_DIRECTION_BACKWARD, base_speed);
        motor_set_left_speed(MOTOR_DIRECTION_BACKWARD, base_speed);
        UART_XDS_PC_DefaultSendLine("Executing: BACKWARD");
    } else if (strstr(data, left_cmd)) {
        motor_set_right_speed(MOTOR_DIRECTION_FORWARD, base_speed / 2);
        motor_set_left_speed(MOTOR_DIRECTION_BACKWARD, base_speed / 2);
        UART_XDS_PC_DefaultSendLine("Executing: LEFT TURN");
    } else if (strstr(data, right_cmd)) {
        motor_set_right_speed(MOTOR_DIRECTION_BACKWARD, base_speed / 2);
        motor_set_left_speed(MOTOR_DIRECTION_FORWARD, base_speed / 2);
        UART_XDS_PC_DefaultSendLine("Executing: RIGHT TURN");
    } else {
        cmd_executed = false;
        UART_XDS_PC_DefaultSendLine("Unknown command received.");
    }

    // 如果找到了ID，则发送响应
    if (strlen(request_id) > 0) {
        if (cmd_executed) {
            OneNet_SendPropertySetResponse(request_id, 200, "success");
            UART_XDS_PC_DefaultSendLine("Sent success response to OneNet.");
        } else {
            OneNet_SendPropertySetResponse(request_id, 500, "failed: unknown command");
            UART_XDS_PC_DefaultSendLine("Sent failure response to OneNet.");
        }
    }
}


/**
 * @brief 系统初始化函数
 */
void SystemInit(void) {
    // 调用系统配置初始化
    SYSCFG_DL_init();

    // !!! 关键修正：手动使能UART2在NVIC中的中断 !!!
    // SysConfig生成的代码可能遗漏了这一步，我们在此手动确保它被执行。
    NVIC_EnableIRQ(UART_4G_GPS_INST_INT_IRQN);
    
    // 初始化时钟
    SysTick_Init();
    
    // 初始化调试串口
    UART_XDS_PC_InitDefault();
    UART_XDS_PC_DefaultSendLine("ICM20948 Test Starting...");

    // 初始化电机
    motor_init();
    motor_enable_system(); // 启用电机驱动
    UART_XDS_PC_DefaultSendLine("Motor System Initialized and Enabled");

    // 初始化偏航角的PID控制器
    // 注意：Kp, Ki, Kd为初始值，需要根据实际情况进行调试以获得最佳直线行驶效果
    PID_Init(&yaw_pid, 2.5f, 0.0f, 1.0f, 0.1f); // Kp, Ki, Kd, 采样时间 (100ms)
    // 设置输出限制，这是对基础速度的修正量
    PID_SetOutputLimits(&yaw_pid, -200, 200); 
    UART_XDS_PC_DefaultSendLine("Yaw PID Controller Initialized");
    
    // 初始化OLED显示屏
    SH1106_Init();
    UART_XDS_PC_DefaultSendLine("OLED Initialized");
    
    // 初始化4G模块
    UART_4G_Init();
    UART_4G_SetDataCallback(OneNet_Command_Handler);
    UART_XDS_PC_DefaultSendLine("4G Module Initialized");
    
    // 初始化OneNet库
    OneNet_Init();
    OneNet_SetSendCallback(OneNet_SendCallback);
    UART_XDS_PC_DefaultSendLine("OneNet Library Initialized");
    
    // 初始化ICM20948
    if (ICM20948_Init() == 0) {
        UART_XDS_PC_DefaultSendLine("ICM20948 Initialized Successfully");
    } else {
        UART_XDS_PC_DefaultSendLine("ICM20948 Initialization Failed!");
        // 显示错误信息在OLED上
        SH1106_Clear();
        SH1106_DrawString(0, 0, "ICM20948 Init", FONT_SIZE_6x8);
        SH1106_DrawString(0, 1, "Failed!", FONT_SIZE_6x8);
        SH1106_Display();
        
        // 无限循环，停止程序
        while (1) {
            mspm0_delay_ms(1000);
        }
    }
}

/**
 * @brief 根据偏航角（Yaw）的PID输出来更新电机，以保持直线行驶
 * @param eulerAngles 包含当前姿态角的结构体指针
 */
void UpdateYawSteeringControl(EulerAngles_t *eulerAngles) {
    // 首次调用时，将当前的偏航角设为目标方向
    if (!yaw_setpoint_initialized && g_calibration.calibrated) {
        yaw_setpoint = eulerAngles->yaw;
        yaw_setpoint_initialized = true;
    }

    // 处理偏航角环绕问题（-180/180度跳变）
    float error = yaw_setpoint - eulerAngles->yaw;
    if (error > 180.0f) {
        error -= 360.0f;
    } else if (error < -180.0f) {
        error += 360.0f;
    }

    // PID控制器根据误差计算修正量
    float pid_output = PID_Update(&yaw_pid, 0, -error); // 我们直接使用误差，所以设定点为0

    // 根据PID修正量计算左右轮速度
    int16_t right_speed = base_speed + (int16_t)pid_output;
    int16_t left_speed  = base_speed - (int16_t)pid_output;

    // 限制速度范围在0到最大值之间
    if (right_speed < 0) right_speed = 0;
    if (right_speed > MOTOR_PWM_MAX_DUTY) right_speed = MOTOR_PWM_MAX_DUTY;
    if (left_speed < 0) left_speed = 0;
    if (left_speed > MOTOR_PWM_MAX_DUTY) left_speed = MOTOR_PWM_MAX_DUTY;

    // 设置电机速度
    motor_set_right_speed(MOTOR_DIRECTION_FORWARD, (uint16_t)right_speed);
    motor_set_left_speed(MOTOR_DIRECTION_FORWARD, (uint16_t)left_speed);

    // 通过串口输出PID调试信息，方便调节参数
    char pid_debug[64];
    snprintf(pid_debug, sizeof(pid_debug), "Yaw:%.1f, Err:%.1f, Out:%.1f, L:%d, R:%d\r\n", 
             eulerAngles->yaw, error, pid_output, left_speed, right_speed);
    UART_XDS_PC_DefaultSendString(pid_debug);
}

/**
 * @brief 传感器校准函数
 * 在传感器静置水平状态下测量偏移量
 */
void CalibrateSensors(void) {
    // 显示校准屏幕
    DisplayCalibrationScreen();
    
    // 等待传感器稳定
    mspm0_delay_ms(500);
    
    // 采集样本数量
    const int numSamples = 50;
    float accel_x_sum = 0, accel_y_sum = 0, accel_z_sum = 0;
    float gyro_x_sum = 0, gyro_y_sum = 0, gyro_z_sum = 0;
    
    UART_XDS_PC_DefaultSendLine("Starting sensor calibration...");
    
    // 采集多个样本并计算平均值
    for (int i = 0; i < numSamples; i++) {
        ICM20948_Data_t data;
        ICM20948_ReadData(&data);
        
        // 累加数据
        accel_x_sum += data.accel_x_g;
        accel_y_sum += data.accel_y_g;
        accel_z_sum += data.accel_z_g;
        gyro_x_sum += data.gyro_x_dps;
        gyro_y_sum += data.gyro_y_dps;
        gyro_z_sum += data.gyro_z_dps;
        
        // 显示进度
        char progressBuffer[32];
        snprintf(progressBuffer, sizeof(progressBuffer), "Progress: %d%%", (i + 1) * 100 / numSamples);
        SH1106_DrawString(20, 5, progressBuffer, FONT_SIZE_6x8);
        SH1106_Display();
        
        mspm0_delay_ms(20);
    }
    
    // 计算平均值
    g_calibration.accel_x_offset = accel_x_sum / numSamples;
    g_calibration.accel_y_offset = accel_y_sum / numSamples;
    // 对于Z轴，我们需要减去1g (假设为标准重力加速度)
    g_calibration.accel_z_offset = accel_z_sum / numSamples - 1.0f;
    g_calibration.gyro_x_offset = gyro_x_sum / numSamples;
    g_calibration.gyro_y_offset = gyro_y_sum / numSamples;
    g_calibration.gyro_z_offset = gyro_z_sum / numSamples;
    
    // 计算静态欧拉角偏移
    float accel_roll = atan2f(g_calibration.accel_y_offset, 
                             g_calibration.accel_z_offset + 1.0f) * 180.0f / M_PI;
    float accel_pitch = atan2f(-g_calibration.accel_x_offset, 
                             sqrtf(g_calibration.accel_y_offset * g_calibration.accel_y_offset + 
                                  (g_calibration.accel_z_offset + 1.0f) * (g_calibration.accel_z_offset + 1.0f))) * 180.0f / M_PI;
                                  
    g_calibration.roll_offset = accel_roll;
    g_calibration.pitch_offset = accel_pitch;
    g_calibration.yaw_offset = 0; // 使用当前朝向作为参考点
    g_calibration.calibrated = true;
    
    // 输出校准结果
    char buffer[128];
    snprintf(buffer, sizeof(buffer), 
             "Calibration done:\nA_offs: %.3f %.3f %.3f\nG_offs: %.3f %.3f %.3f",
             g_calibration.accel_x_offset, g_calibration.accel_y_offset, g_calibration.accel_z_offset,
             g_calibration.gyro_x_offset, g_calibration.gyro_y_offset, g_calibration.gyro_z_offset);
    UART_XDS_PC_DefaultSendLine(buffer);
    
    // 显示校准完成
    SH1106_Clear();
    SH1106_DrawString(15, 3, "Calib Done!", FONT_SIZE_6x8);
    SH1106_Display();
    mspm0_delay_ms(1000);
}

/**
 * @brief 计算欧拉角
 * @param sensorData 传感器数据
 * @param eulerAngles 欧拉角结构体指针
 */
void CalculateEulerAngles(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles) {
    // 静态变量声明 - 移至函数开头
    static float gyroRoll = 0, gyroPitch = 0, gyroYaw = 0;
    static uint32_t lastTime = 0;
    static uint32_t lastDebugTime = 0;
    static uint32_t stableStartTime = 0;
    static bool inStableState = false;
    static bool firstRun = true;
    
    // 加速度计原始数据转换为角度
    float accelRoll = atan2f(sensorData->accel_y_g, sensorData->accel_z_g) * 180.0f / M_PI;
    float accelPitch = atan2f(-sensorData->accel_x_g, 
                             sqrtf(sensorData->accel_y_g * sensorData->accel_y_g + 
                                  sensorData->accel_z_g * sensorData->accel_z_g)) * 180.0f / M_PI;
    
    // 陀螺仪角速度积分
    uint32_t currentTime = sensorData->timestamp;
    float dt = (lastTime == 0) ? 0.01f : (currentTime - lastTime) / 1000.0f;
    if (dt <= 0) dt = 0.01f;  // 防止除零错误
    lastTime = currentTime;
    
    // 首次调用时初始化陀螺仪积分值
    if (gyroRoll == 0 && gyroPitch == 0 && gyroYaw == 0) {
        gyroRoll = accelRoll;
        gyroPitch = accelPitch;
        gyroYaw = 0;  // 初始化为0
    }
    
    // 检测静止状态 - 可减少漂移
    float accelMagnitude = sqrtf(sensorData->accel_x_g * sensorData->accel_x_g +
                                sensorData->accel_y_g * sensorData->accel_y_g +
                                sensorData->accel_z_g * sensorData->accel_z_g);
    
    float gyroMagnitude = sqrtf(sensorData->gyro_x_dps * sensorData->gyro_x_dps +
                               sensorData->gyro_y_dps * sensorData->gyro_y_dps +
                               sensorData->gyro_z_dps * sensorData->gyro_z_dps);
    
    // 静止检测阈值
    const float ACCEL_STABLE_THRESHOLD = 0.05f;   // 加速度稳定阈值 (g)
    const float GYRO_STABLE_THRESHOLD = 0.5f;     // 陀螺稳定阈值 (dps)
    
    // 判断是否静止
    bool isStable = (fabsf(accelMagnitude - 1.0f) < ACCEL_STABLE_THRESHOLD) && 
                    (gyroMagnitude < GYRO_STABLE_THRESHOLD);
    
    // 自适应互补滤波系数 - 动态调整
    // 在剧烈运动时更信任陀螺仪，在静止时更信任加速度计
    float alpha = 0.98f;  // 默认值
    
    if (isStable) {
        // 设备静止时降低陀螺仪权重，减少漂移
        alpha = 0.7f;
        
        // 在长时间静止状态下逐渐校准陀螺仪零点
        if (!inStableState) {
            stableStartTime = currentTime;
            inStableState = true;
        } else if ((currentTime - stableStartTime) > 3000) { // 静止超过3秒
            // 缓慢衰减陀螺仪积分的偏航角偏差，但保留Roll和Pitch
            // 因为偏航角容易漂移
            if (fabsf(gyroYaw - 0.0f) > 0.1f) {
                gyroYaw = gyroYaw * 0.99f; // 缓慢衰减到零
            }
        }
    } else {
        // 在运动状态下更信任陀螺仪
        alpha = 0.98f;
        inStableState = false;
    }
    
    // 动态调整陀螺仪读数，在静止时趋向于零，减少漂移
    float gyro_x = sensorData->gyro_x_dps;
    float gyro_y = sensorData->gyro_y_dps;
    float gyro_z = sensorData->gyro_z_dps;
    
    // 设置陀螺仪死区，消除小噪声
    const float GYRO_DEADBAND = 0.2f; // 死区阈值，低于此值视为0
    if (fabsf(gyro_x) < GYRO_DEADBAND) gyro_x = 0;
    if (fabsf(gyro_y) < GYRO_DEADBAND) gyro_y = 0;
    if (fabsf(gyro_z) < GYRO_DEADBAND) gyro_z = 0;
    
    // 更新陀螺仪积分
    gyroRoll += gyro_x * dt;
    gyroPitch += gyro_y * dt;
    gyroYaw += gyro_z * dt;
    
    // 互补滤波，结合加速度计和陀螺仪数据
    eulerAngles->roll = alpha * (eulerAngles->roll + gyro_x * dt) + (1.0f - alpha) * accelRoll;
    eulerAngles->pitch = alpha * (eulerAngles->pitch + gyro_y * dt) + (1.0f - alpha) * accelPitch;
    
    // 偏航角只由陀螺仪积分得到
    eulerAngles->yaw += gyro_z * dt;
    
    // 将偏航角限制在0-360度范围内
    while (eulerAngles->yaw >= 360.0f) eulerAngles->yaw -= 360.0f;
    while (eulerAngles->yaw < 0.0f) eulerAngles->yaw += 360.0f;
    
    // 重置偏航角为0，因为我们只关心相对转动
    if (firstRun && g_calibration.calibrated) {
        eulerAngles->roll = 0;
        eulerAngles->pitch = 0;
        eulerAngles->yaw = 0;
        firstRun = false;
    }
    
    // 调试输出
    if ((currentTime - lastDebugTime) > 1000) { // 每秒输出一次
        lastDebugTime = currentTime;
        if (isStable) {
            UART_XDS_PC_DefaultSendLine("Status: STABLE");
        }
    }
}

/**
 * @brief 显示传感器数据到OLED
 * @param sensorData 传感器数据
 * @param eulerAngles 欧拉角数据
 */
void DisplaySensorData(ICM20948_Data_t *sensorData, EulerAngles_t *eulerAngles) {
    char buffer[32];
    
    // 清屏
    SH1106_Clear();
    
    // 显示标题
    SH1106_DrawString(0, 0, "ICM20948 Data", FONT_SIZE_6x8);
    SH1106_DrawLine(0, 10, 127, 10, 1);
    
    // 显示欧拉角
    SH1106_DrawString(0, 2, "Euler Angles:", FONT_SIZE_6x8);
    
    snprintf(buffer, sizeof(buffer), "Roll : %5.1f", eulerAngles->roll);
    SH1106_DrawString(0, 3, buffer, FONT_SIZE_6x8);
    
    snprintf(buffer, sizeof(buffer), "Pitch: %5.1f", eulerAngles->pitch);
    SH1106_DrawString(0, 4, buffer, FONT_SIZE_6x8);
    
    snprintf(buffer, sizeof(buffer), "Yaw  : %5.1f", eulerAngles->yaw);
    SH1106_DrawString(0, 5, buffer, FONT_SIZE_6x8);
    
    // 显示加速度数据
    snprintf(buffer, sizeof(buffer), "Ax:%4.2f Ay:%4.2f", sensorData->accel_x_g, sensorData->accel_y_g);
    SH1106_DrawString(0, 6, buffer, FONT_SIZE_6x8);
    
    // 显示温度
    snprintf(buffer, sizeof(buffer), "Temp: %5.1fC", sensorData->temp_c);
    SH1106_DrawString(0, 7, buffer, FONT_SIZE_6x8);
    
    // 更新显示
    SH1106_Display();
}

/**
 * @brief 显示启动画面
 */
void DisplayStartupScreen(void) {
    SH1106_Clear();
    
    // 显示欢迎信息
    SH1106_DrawString(20, 1, "ICM20948 TEST", FONT_SIZE_6x8);
    SH1106_DrawString(10, 3, "9-Axis Motion Sensor", FONT_SIZE_6x8);
    SH1106_DrawString(20, 5, "Initializing...", FONT_SIZE_6x8);
    
    // 绘制边框
    SH1106_DrawRectangle(0, 0, 128, 64, 1);
    
    // 更新显示
    SH1106_Display();
    
    // 等待一段时间显示欢迎界面
    mspm0_delay_ms(2000);
}

/**
 * @brief 显示校准屏幕
 */
void DisplayCalibrationScreen(void) {
    SH1106_Clear();
    
    // 显示校准信息
    SH1106_DrawString(15, 1, "Sensor Calib", FONT_SIZE_6x8);
    SH1106_DrawString(0, 3, "Keep device still", FONT_SIZE_6x8);
    SH1106_DrawString(20, 5, "Calibrating...", FONT_SIZE_6x8);
    
    // 绘制边框
    SH1106_DrawRectangle(0, 0, 128, 64, 1);
    
    // 更新显示
    SH1106_Display();
}